#include "protocal.h"
#include "common.h"
#include <stdint.h>

#include "log.h"
#include "app_uart.h"

#define MAX_DATA_LEN 30

#define BYTE1(x) ((uint8_t)(x >> 8 & 0xff))
#define BYTE0(x) ((uint8_t)(x&0xff))



unsigned char data_to_send[MAX_DATA_LEN];

void Uart1_Put_Buf(uint8_t *data, uint8_t count)
{
    for(uint8_t i = 0; i < count; i++)
        app_uart_put(*data++);
//        fput_c(*data++);
}

//void send_Accel_Gyro(Accel *acc, Gyro *gyro)
//{
//    uint8_t _cnt=0;
//    data_to_send[_cnt++]=0xAA;
//    data_to_send[_cnt++]=0xAA;
//    data_to_send[_cnt++]=0x02;
//    data_to_send[_cnt++]=0;
//    data_to_send[_cnt++]=BYTE1(acc->X);
//    data_to_send[_cnt++]=BYTE0(acc->X);
//    data_to_send[_cnt++]=BYTE1(acc->Y);
//    data_to_send[_cnt++]=BYTE0(acc->Y);
//    data_to_send[_cnt++]=BYTE1(acc->Z);
//    data_to_send[_cnt++]=BYTE0(acc->Z);
//    data_to_send[_cnt++]=BYTE1(gyro->X);
//    data_to_send[_cnt++]=BYTE0(gyro->X);
//    data_to_send[_cnt++]=BYTE1(gyro->Y);
//    data_to_send[_cnt++]=BYTE0(gyro->Y);
//    data_to_send[_cnt++]=BYTE1(gyro->Z);
//    data_to_send[_cnt++]=BYTE0(gyro->Z);
//    data_to_send[_cnt++]=0;
//    data_to_send[_cnt++]=0;
//    data_to_send[_cnt++]=0;
//    data_to_send[_cnt++]=0;
//    data_to_send[_cnt++]=0;
//    data_to_send[_cnt++]=0;

//    data_to_send[3] = _cnt-4;

//    uint8_t sum = 0;
//    for(uint8_t i=0;i<_cnt;i++)
//        sum += data_to_send[i];
//    data_to_send[_cnt++] = sum;

//    Uart1_Put_Buf(data_to_send,_cnt);
//}

void send_Accel_Gyro(short *acc, short *gyro)
{
    uint8_t _cnt=0;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0x02;
    data_to_send[_cnt++]=0;
    data_to_send[_cnt++]=BYTE1(acc[0]);
    data_to_send[_cnt++]=BYTE0(acc[0]);
    data_to_send[_cnt++]=BYTE1(acc[1]);
    data_to_send[_cnt++]=BYTE0(acc[1]);
    data_to_send[_cnt++]=BYTE1(acc[2]);
    data_to_send[_cnt++]=BYTE0(acc[2]);
    data_to_send[_cnt++]=BYTE1(gyro[0]);
    data_to_send[_cnt++]=BYTE0(gyro[0]);
    data_to_send[_cnt++]=BYTE1(gyro[1]);
    data_to_send[_cnt++]=BYTE0(gyro[1]);
    data_to_send[_cnt++]=BYTE1(gyro[2]);
    data_to_send[_cnt++]=BYTE0(gyro[2]);
    data_to_send[_cnt++]=0;
    data_to_send[_cnt++]=0;
    data_to_send[_cnt++]=0;
    data_to_send[_cnt++]=0;
    data_to_send[_cnt++]=0;
    data_to_send[_cnt++]=0;

    data_to_send[3] = _cnt-4;

    uint8_t sum = 0;
    for(uint8_t i=0;i<_cnt;i++)
        sum += data_to_send[i];
    data_to_send[_cnt++] = sum;

    Uart1_Put_Buf(data_to_send,_cnt);
}

void send_Angle(Eulerian_angle *angle)
{
    uint8_t _cnt=0;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0x01;
    data_to_send[_cnt++]=0;
    int16_t _temp;
    _temp = (int)(angle->roll*100);
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);
    _temp = (int)(angle->pitch*100);
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);
    //_temp = (int)(Q_ANGLE.YAW*100);
    _temp = (int)(angle->yaw*100);
    data_to_send[_cnt++]=BYTE1(_temp);
    data_to_send[_cnt++]=BYTE0(_temp);
//    _temp = Alt_CSB;
//    data_to_send[_cnt++]=BYTE1(_temp);
//    data_to_send[_cnt++]=BYTE0(_temp);
//    int32_t _temp2 = Alt;
//    data_to_send[_cnt++]=BYTE3(_temp2);
//    data_to_send[_cnt++]=BYTE2(_temp2);
//    data_to_send[_cnt++]=BYTE1(_temp2);
//    data_to_send[_cnt++]=BYTE0(_temp2);
    data_to_send[_cnt++] = 0x00;
    data_to_send[_cnt++] = 0x00;
    data_to_send[_cnt++] = 0x00;
    data_to_send[_cnt++] = 0x00;
    data_to_send[_cnt++] = 0x00;
    data_to_send[_cnt++] = 0x00;
    
    data_to_send[_cnt++]=0xA0;
    
//    if(Rc_C.ARMED==0)        data_to_send[_cnt++]=0xA0;    
//    else if(Rc_C.ARMED==1)        data_to_send[_cnt++]=0xA1;

    data_to_send[3] = _cnt-4;

    uint8_t sum = 0;
    for(uint8_t i=0; i<_cnt; i++)
        sum += data_to_send[i];
    data_to_send[_cnt++]=sum;

    Uart1_Put_Buf(data_to_send,_cnt);
}



void sendData2UpperMonitor(short *gyro, short *accel, long *quat)
{
    
}


void send_Accel_Gyro_1(short a_x, short a_y, short a_z, short g_x, short g_y, short g_z)
{
//    short tmp = KalmanFilter_x(acc[0],KALMAN_Q,KALMAN_R);
    
    
    uint8_t _cnt=0;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0xAA;
    data_to_send[_cnt++]=0x02;
    data_to_send[_cnt++]=0;
    data_to_send[_cnt++]=BYTE1((uint16_t)a_x);
    data_to_send[_cnt++]=BYTE0((uint16_t)a_x);
    data_to_send[_cnt++]=BYTE1(a_y);
    data_to_send[_cnt++]=BYTE0(a_y);
    data_to_send[_cnt++]=BYTE1(a_z);
    data_to_send[_cnt++]=BYTE0(a_z);
   
    data_to_send[_cnt++]=BYTE1(g_x);
    data_to_send[_cnt++]=BYTE0(g_x);
    data_to_send[_cnt++]=BYTE1(g_y);
    data_to_send[_cnt++]=BYTE0(g_y);
    data_to_send[_cnt++]=BYTE1(g_z);
    data_to_send[_cnt++]=BYTE0(g_z);
    data_to_send[_cnt++]=0;
    data_to_send[_cnt++]=0;
    data_to_send[_cnt++]=0;
    data_to_send[_cnt++]=0;
    data_to_send[_cnt++]=0;
    data_to_send[_cnt++]=0;

    data_to_send[3] = _cnt-4;

    uint8_t sum = 0;
    for(uint8_t i=0;i<_cnt;i++)
        sum += data_to_send[i];
    data_to_send[_cnt++] = sum;

    Uart1_Put_Buf(data_to_send,_cnt);
}
